package roborally.util;
import java.util.*;
import be.kuleuven.cs.som.annotate.*;

import roborally.Board;
import roborally.Pieces.*;
/**
 * This is a class of Dijkstra Paths that have a path of PositionWithOrientations through which a Robot can go, a Manhattan distance 
 * representing a distance to be minimized and an amount of Energy needed to take the path.
 * @version 3.0
 * @author Bart Opsomer 2de Bachelor Ingenieurswetenschappen Electrotechniek-Computerwetenschappen 
 * 		   Peter Opsomer 1ste Master Wiskundige IngenieursTechnieken
 */
public class DijkstraPath implements Comparable<DijkstraPath> {
	// The manhattanDistance to the desired location
	private long manhattanDistance;
	// The energy to reach the desired location, only at the end it is the correct value
	private Energy requiredEnergy;
	// The path to minimize the manhattanDistance, for the robot
	private List<PositionWithOrientation> path;
	/**
	 * Create a default DijkstraPath to start from.
	 * @param manhattanDistance the Manhattan Distance to the desired location before optimization
	 * @post  |  new.getManhattanDistance() == manhattanDistance
	 * @post  |  new.getEnergy().compareTo(Energy.ZERO) == 0
	 * @post  |  new.getPath().isEmpty()
	 */
	@Raw
	public DijkstraPath(long manhattanDistance) {
		this.manhattanDistance = manhattanDistance;
		requiredEnergy = new Energy(0);
		path = new ArrayList<PositionWithOrientation>();
	}
	/**
	 * Creates a new DijkstraPath from the previous one by extending it to going to the next layer 'goodNeighbour'
	 * @param manhattanDistance the Manhattan Distance to the desired location during optimization
	 * @param previous the previous DijkstraPath to be extended
	 * @param goodNeighbour the PositionWithOrientation to extend 'previous' with
	 * @param pos the previous PositionWithOrientation, so the last PositionWithOrientation in previous.getPath()
	 * @param moveEnergy the energy required to move of the Robot that could have to execute this path
	 * @param turnEnergy the energy required to turn of the Robot that could have to execute this path
	 * @post  |  new.getManhattanDistance() == manhattanDistance
	 * @post  |  new.getEnergy().compareTo(previous.getEnergy() ) > 0
	 * @post  |  new.getPath().containsAll(previous.getPath() ) && new.getPath().contains(goodNeighbour)
	 */
	@Raw
	public DijkstraPath(long manhattanDistance,DijkstraPath previous,PositionWithOrientation goodNeighbour,PositionWithOrientation pos,Energy moveEnergy,Energy turnEnergy) {
		this.manhattanDistance = manhattanDistance;
		requiredEnergy = new Energy(previous.requiredEnergy);
		requiredEnergy.increase(moveEnergy);
		if( (!previous.path.isEmpty() ) && (previous.path.get(previous.path.size() - 1) != pos) ) {
			throw new IllegalStateException("Previous position should be last in path");
		}
		int orientationDifference = Math.abs(goodNeighbour.getOrientation().getOrientationDifference(pos.getOrientation() ) );
		if(orientationDifference == 2) {
			requiredEnergy.increase(turnEnergy);
			requiredEnergy.increase(turnEnergy);
		}
		else if(orientationDifference == 1) {
			requiredEnergy.increase(turnEnergy);
		} // else nothing
		path = new ArrayList<PositionWithOrientation>(previous.path);
		path.add(goodNeighbour);
	}
	/**
	 * @return the Manhattan Distance to the desired location during optimization
	 */
	@Basic
	public long getManhattanDistance() {
		return manhattanDistance;
	}
	/**
	 * @return the energy the Robot corresponding this DijkstraPath has to use to get through getPath()
	 */
	@Basic
	public Energy getEnergy() {
		return requiredEnergy;
	}
	/**
	 * @return the path of PositionWithOrientations through which the Robot corresponding this DijkstraPath can go
	 */
	@Basic
	public List<PositionWithOrientation> getPath() {
		return path;
	}
	/**
	 * Get whether this DijkstraPath requires less Energy of its Robot to be executed than the other
	 * @param other the DijkstraPath to compare the energy with
	 * @return |  other.requiredEnergy.isGreaterThanOrEquals(requiredEnergy)
	 */
	private boolean requiresLessOrSameEnergy(DijkstraPath other) {
		return other.requiredEnergy.isGreaterThanOrEquals(requiredEnergy);
	}
	/**
	 * Returns negative number if this is 'less than' other, meaning it is closer to the end position or is as close but requires less energy.
	 * @param other the DijkstraPath to compare this with
	 * @return  |	if (other.getManhattanDistance() > this.getManhattanDistance() ) then
	 * 			|		result < 0
	 * @return  |	if (other.getManhattanDistance() < this.getManhattanDistance() ) then
	 * 			|		result > 0
	 * @return  |	if (other.getManhattanDistance() == this.getManhattanDistance() && getEnergy().isGreaterThanOrEquals(other.getEnergy() ) &&
	 * 			|	!getEnergy().equals(other.getEnergy() ) then
	 * 			|		result > 0
	 * @return  |	if (other.getManhattanDistance() == this.getManhattanDistance() && other.getEnergy().isGreaterThanOrEquals(getEnergy() ) &&
	 * 			|	!getEnergy().equals(other.getEnergy() ) then
	 * 			|		result < 0
	 */
	public int compareTo(DijkstraPath other) {
		if(manhattanDistance == other.manhattanDistance) {
			if(requiredEnergy.isGreaterThanOrEquals(other.requiredEnergy)) {
				return +1;
			}
			return -1;
		}
		return Math.round(manhattanDistance - other.manhattanDistance);
	}
	/**
	 * Return the optimal DijkstraPath to get from 'start' to 'toReach' when the energy required to move and turn are given.
	 * @param start		the PositionWithOrientation to start from
	 * @param toReach	the Position to reach
	 * @param moveEnergy	the energy required to move
	 * @param turnEnergy	the energy required to turn
	 * @return	the DijkstraPath with minimal energy that is needed to get from 'start' to 'toReach', taking into account Walls and other
	 * 			Robots (that cannot move) when the energy required to move and turn are given
	 * 			|  !exists List<PositionWithOrientation> list and Energy energy initially Energy.ZERO with
	 * 			|		list.get(0).equals(start) && list.get(list.size() ).getPosition().equals(toReach) &&
	 * 			|		for i = 1..list.size() -1
	 * 			|			(list.get(i).getPosition().getManhattanDistanceTo(list.get(i-1).getPosition() ) == 1) &&
	 * 			|			!list.get(i).getPosition().getBoard().hasRobotAt(list.get(i).getPosition() ) && 
	 * 			|			!list.get(i).getPosition().getBoard().hasWallAt(list.get(i).getPosition() ) 
	 * 			|			energy.increase(moveEnergy)
	 * 			|			energy.increase(new Energy(turnEnergy.getAmount()*list.get(i).getOrientation().
	 * 			|				getOrientationDifference(list.get(i-1).getOrientation() ) ) )
	 * 			|  and result.getEnergy() > energy
	 */
	public static DijkstraPath getOptimalPathTo(PositionWithOrientation start, Position toReach, Energy moveEnergy, Energy turnEnergy) {
		Board board = start.getBoard();
		if(!board.equals(toReach.getBoard()) ) {
			throw new IllegalArgumentException();
		}
		DijkstraPath optimal = new DijkstraPath(start.getPosition().getManhattanDistanceTo(toReach));
		Map<PositionWithOrientation,DijkstraPath> previous = new HashMap<PositionWithOrientation,DijkstraPath>();
		Map<PositionWithOrientation,DijkstraPath> current = new HashMap<PositionWithOrientation,DijkstraPath>();
		Map<PositionWithOrientation,DijkstraPath> next = new HashMap<PositionWithOrientation,DijkstraPath>();
		next.put(start, optimal);
		Energy leastEnergyInNext = new Energy(Double.MAX_VALUE);
		//TODO lusinvariant
		
		// kan zijn dat optimaler om langere afstand af te leggen als daardoor veel minder te draaien: ofwel door een extra aantal keer te itereren en dit
		// extra keer is (hoog aantal keer draaien van opt [evt - laag aantal keer draaien van minste])*turnEnergy/moveEnergy
		// Dus verder blijven zoeken als er nog nieuwe posities zijn in de volgende laag en als in next nog paden zitten met minder energie (dan wel in 
		// meeste gevallen nutteloos aan het zoeken) of als eindpositie bereikt.
		while( (optimal.manhattanDistance != 0 || optimal.getEnergy().isGreaterThanOrEquals(leastEnergyInNext) ) && (!next.isEmpty() ) ) {
			previous = current;
			current = next;
			next = new HashMap<PositionWithOrientation,DijkstraPath>();
			Set<PositionWithOrientation> notThese = new HashSet<PositionWithOrientation>(previous.keySet());
			notThese.addAll(current.keySet());
			leastEnergyInNext = new Energy(Double.MAX_VALUE);
			// Go over the current layer to find the next one
			for(PositionWithOrientation pos: current.keySet()) {
				// Check all neigbours of the current layer,that are valid,not check and not containing Walls or Robots
				for(PositionWithOrientation goodNeighbour : pos.getGoodNeighbours(notThese)) {
					DijkstraPath pathToGoodNeighbour = new DijkstraPath(goodNeighbour.getPosition().getManhattanDistanceTo(toReach),current.get(pos),goodNeighbour,pos,
							moveEnergy,turnEnergy);
					if(pathToGoodNeighbour.compareTo(optimal) < 0) {
						optimal = pathToGoodNeighbour;
					}
					if(leastEnergyInNext.isGreaterThanOrEquals(pathToGoodNeighbour.requiredEnergy) ) {
						leastEnergyInNext = pathToGoodNeighbour.requiredEnergy;
					}
					if(!next.containsKey(goodNeighbour)) {
						next.put(goodNeighbour,pathToGoodNeighbour);
					} // Override if more optimal
					else if(pathToGoodNeighbour.requiresLessOrSameEnergy(next.get(goodNeighbour) ) ){ 
					//else if(pathToGoodNeighbour.compareTo(next.get(goodNeighbour) ) > 0){
						next.put(goodNeighbour, pathToGoodNeighbour);
					}
				}
			}
		}
		return optimal;
	}
	/**
	 * Get the optimal paths of two Robots to get as close to each other as possible with their given Energies using as least energy as possible.
	 * In this method, all DijkstraPaths are defined with abs(getManhattanDistance() - 1) so that optimally 0 when 1 position apart. 
	 * result[0] gives the path the first Robot should follow, result[1] for second Robot
	 * @param start the start PositionWithOrientations of both Robots
	 * @param moveEnergy the energies required to move of both Robots
	 * @param turnEnergy the energies required to turn of both Robots
	 * @param robotEnergy the energies of both Robots
	 * @return	No two path exist for both Robots in which they can get closer or to the same Manhattan Distance in less energy
	 * 			|  !exists List<PositionWithOrientation> list and listRobot and Energy energy initially Energy.ZERO with
	 * 			|		list.get(0).equals(start[0]) && listRobot.get(0).equals(start[1]) &&
	 * 			|		for i = 1..list.size() -1
	 * 			|			(list.get(i).getPosition().getManhattanDistanceTo(list.get(i-1).getPosition() ) == 1) &&
	 * 			|			!list.get(i).getPosition().getBoard().hasRobotAt(list.get(i).getPosition() ) && 
	 * 			|			!list.get(i).getPosition().getBoard().hasWallAt(list.get(i).getPosition() ) 
	 * 			|			energy.increase(moveEnergy[0])
	 * 			|			energy.increase(new Energy(turnEnergy[0].getAmount()*list.get(i).getOrientation().
	 * 			|				getOrientationDifference(list.get(i-1).getOrientation() ) ) )
	 * 			|		for i = 1..listRobot.size() -1
	 * 			|			(listRobot.get(i).getPosition().getManhattanDistanceTo(listRobot.get(i-1).getPosition() ) == 1) &&
	 * 			|			!listRobot.get(i).getPosition().getBoard().hasRobotAt(listRobot.get(i).getPosition() ) && 
	 * 			|			!listRobot.get(i).getPosition().getBoard().hasWallAt(listRobot.get(i).getPosition() ) 
	 * 			|			energy.increase(moveEnergy[1])
	 * 			|			energy.increase(new Energy(turnEnergy[1].getAmount()*listRobot.get(i).getOrientation().
	 * 			|				getOrientationDifference(listRobot.get(i-1).getOrientation() ) ) )
	 * 			|  and (list.get(list.size()-1 ).manhattanDistanceTo(listRobot.get(listRobot.size() ) ) < 
	 * 			|		result[0].getPath().get(result[0].getPath().size() - 1).getPosition().getManhattanDistanceTo(
				|		result[1].getPath().get(result[1].getPath().size() - 1).getPosition())
	 * 			|  or [ (list.get(list.size()-1 ).manhattanDistanceTo(listRobot.get(listRobot.size() ) ) == 
	 * 			|		result[0].getPath().get(result[0].getPath().size() - 1).getPosition().getManhattanDistanceTo(
				|		result[1].getPath().get(result[1].getPath().size() - 1).getPosition())
	 * 			|  and  result[0].getEnergy().increase(result[1].getEnergy() ) > energy ]
	 */
	public static DijkstraPath[] getOptimalPathsToReachEachOther(PositionWithOrientation[] start, Energy moveEnergy[], Energy turnEnergy[], Energy robotEnergy[]) {
		DijkstraPath optimal[] = new DijkstraPath[2];
		// Initially, optimal is that neither Robot moves
		optimal[0] = new DijkstraPath(Math.abs(start[0].getPosition().getManhattanDistanceTo(start[1].getPosition() ) - 1) );
		optimal[1] = new DijkstraPath(optimal[0].getManhattanDistance() );
		// So first get all PWOs that both Robots can reach
		Map<PositionWithOrientation,DijkstraPath> reachableByFirstRobot = getOptimalPathsToAllReachable(start[0], start[1].getPosition(), 
				moveEnergy[0], turnEnergy[0], robotEnergy[0], optimal[0]);
		Map<PositionWithOrientation,DijkstraPath> reachableBySecondRobot = getOptimalPathsToAllReachable(start[1], start[0].getPosition(), 
				moveEnergy[1], turnEnergy[1], robotEnergy[1], optimal[1]);
		// The minimal distance attained yet: first minimize this and only then energy so initialize energy at 0
		long minDistance = optimal[0].getManhattanDistance();
		Energy minimalTotal = optimal[0].requiredEnergy; // + optimal[1].requiredEnergy but zero anyway
		// iterations over all combinations of reachable positions to get the (global) minimum DijkstraPath[]
		for(PositionWithOrientation pwo1 : reachableByFirstRobot.keySet()) {
			for(PositionWithOrientation pwo2: reachableBySecondRobot.keySet() ) {
				long distanceBetweenPWOS = Math.abs(pwo1.getPosition().getManhattanDistanceTo(pwo2.getPosition()) - 1);
				Energy total = new Energy(reachableByFirstRobot.get(pwo1).requiredEnergy);
				total.increase(reachableBySecondRobot.get(pwo2).requiredEnergy);
				// If less distance or same distance with less energy
				if(distanceBetweenPWOS < minDistance || (distanceBetweenPWOS == minDistance && minimalTotal.isGreaterThanOrEquals(total) ) ) {
					minDistance = distanceBetweenPWOS;
					optimal[0] = reachableByFirstRobot.get(pwo1);
					optimal[1] = reachableBySecondRobot.get(pwo2);
					minimalTotal = total;
				}
			}
		}
		return optimal;
	}
	/**
	 * Get the DijkstraPaths to all PositionWithOrientations that a Robot can reach optimally with its Energies. This method does not call getOptimalPathTo 
	 * for every PositionWithOrientation since that would require more time; here, we retain the Maps in the method.
	 * @param start 			the PositionWithOrientation to start from
	 * @param toStopWhenReached	the Position when to start stopping the iterations
	 * @param moveEnergy		the Energy required to move
	 * @param turnEnergy		the Energy required to turn
	 * @param robotEnergy		the Energy that can be used maximally
	 * @param dijkstraStart		A pre-initialised DijkstraPath
	 * @return	| !exists PositionWithOrientation pwo where
	 * 			| exists List<PositionWithOrientation> list and Energy energy initially Energy.ZERO with
	 * 			|		list.get(0).equals(start) && list.get(list.size() ).equals(pwo) &&
	 * 			|		for i = 1..list.size() -1
	 * 			|			(list.get(i).getPosition().getManhattanDistanceTo(list.get(i-1).getPosition() ) == 1) &&
	 * 			|			!list.get(i).getPosition().getBoard().hasRobotAt(list.get(i).getPosition() ) && 
	 * 			|			!list.get(i).getPosition().getBoard().hasWallAt(list.get(i).getPosition() ) 
	 * 			|			energy.increase(moveEnergy)
	 * 			|			energy.increase(new Energy(turnEnergy.getAmount()*list.get(i).getOrientation().
	 * 			|				getOrientationDifference(list.get(i-1).getOrientation() ) ) )
	 * 			|  and !result.containsKey(pwo)
	 * 			|  and energy < robotEnergy
	 * 			|  and energy < result.get(pwo).getEnergy()
	 */
	public static Map<PositionWithOrientation,DijkstraPath> getOptimalPathsToAllReachable(PositionWithOrientation start, Position toStopWhenReached, 
			Energy moveEnergy, Energy turnEnergy, Energy robotEnergy,DijkstraPath dijkstraStart) {
		Board board = start.getBoard();
		if(!board.equals(toStopWhenReached.getBoard()) ) {
			throw new IllegalArgumentException();
		}
		Map<PositionWithOrientation,DijkstraPath> previous = new HashMap<PositionWithOrientation,DijkstraPath>();
		Map<PositionWithOrientation,DijkstraPath> current = new HashMap<PositionWithOrientation,DijkstraPath>();
		Map<PositionWithOrientation,DijkstraPath> next = new HashMap<PositionWithOrientation,DijkstraPath>();
		Map<PositionWithOrientation,DijkstraPath> allOptimalPaths = new HashMap<PositionWithOrientation,DijkstraPath>();
		// Only Energy matters in this method but don't put all distances to 0 so that they are meaningful in getOptimalPathsToReachEachOther, although not useful
		next.put(start, dijkstraStart );
		Energy leastEnergyInNext = Energy.ZERO;
		boolean otherReached = false;
		byte extraSteps = 0;
		// Stop loop when no more existing Positions or the Robot does not have enough Energy to reach any of the next positions
		// Also use a few extra iterations because could be that toStopWhenReached is reached but other path with more moves and less 
		// energy is still not completed
		// bound of extraSteps should actually be (max number of turns - min number of turns in next)*turnEnergy/moveEnergy
		while( (extraSteps < 4) && robotEnergy.isGreaterThanOrEquals(leastEnergyInNext) && (!next.isEmpty() ) ) {
			if(otherReached) {
				extraSteps ++;
			}
			previous = current;
			current = next;
			allOptimalPaths.putAll(next);
			next = new HashMap<PositionWithOrientation,DijkstraPath>();
			Set<PositionWithOrientation> notThese = new HashSet<PositionWithOrientation>(previous.keySet());
			notThese.addAll(current.keySet());
			leastEnergyInNext = new Energy(Double.MAX_VALUE);
			// Go over current layer to get next layer
			for(PositionWithOrientation pos: current.keySet()) {
				// Go over all neighbours of the PWO in the current layer that are valid, are not iterated yet and don't have a wall or Robot
				for(PositionWithOrientation goodNeighbour : pos.getGoodNeighbours(notThese)) {
					DijkstraPath pathToGoodNeighbour = new DijkstraPath(goodNeighbour.getPosition().getManhattanDistanceTo(toStopWhenReached),
							current.get(pos),goodNeighbour,pos,moveEnergy,turnEnergy);
					if(leastEnergyInNext.isGreaterThanOrEquals(pathToGoodNeighbour.requiredEnergy) ) {
						leastEnergyInNext = pathToGoodNeighbour.requiredEnergy;
					}
					if(goodNeighbour.getPosition().getManhattanDistanceTo(toStopWhenReached) == 1) {
						otherReached = true;
					}
					if (robotEnergy.isGreaterThanOrEquals(pathToGoodNeighbour.requiredEnergy) && !allOptimalPaths.containsKey(goodNeighbour) ) {
						if (!next.containsKey(goodNeighbour)) {
							next.put(goodNeighbour, pathToGoodNeighbour);
						} 
						else if (pathToGoodNeighbour.requiresLessOrSameEnergy(next.get(goodNeighbour))) {
							next.put(goodNeighbour, pathToGoodNeighbour);
						}
					} // else don't add to next because not enough energy to reach goodNeighbour anyway

					// Extra addition to allOptimalPaths since it may be possible to get to same PWO with longer path and less Energy
					if (allOptimalPaths.containsKey(goodNeighbour) && pathToGoodNeighbour.requiresLessOrSameEnergy(allOptimalPaths.get(goodNeighbour) ) ) {
						allOptimalPaths.put(goodNeighbour, pathToGoodNeighbour);
					}
				}
			}
		}
		return allOptimalPaths;
	}
}
